% clear all;
% close all;

%%Basic definitions
%%We define some constants to be used on the script
f0 = 2e6;       % pulse center frequency [Hz]
cycles=2;       % number of cycles in pulse
c0 = 1540;      % medium speed of sound [m/s]
rho0 = 1020;    % medium density [kg/m3]
F_number = 1.7; % F number for CPWC sequence (i.e. maximum angle)
N=3;            % number of plane waves in CPWC sequence

%%probe
%%We define the ultrasound probe.
% prb=uff.linear_array();
% prb.N=128;                  % number of elements
% prb.pitch=300e-6;           % probe pitch in azimuth [m]
% prb.element_width=300e-6;   % element width [m]
% prb.element_height=5000e-6; % element height [m]
% fig_handle = prb.plot([],'Linear array');

param.fc = f0; % Transducer center frequency [Hz]
param.kerf = 0e-6; % Kerf [m]
param.width = 300e-6; % Width [m]
param.pitch = 300e-6; % Pitch [m]
param.Nelements = 128;
param.bandwidth = 77; % Fractional bandwidth [%]
param.radius = Inf;
param.height = 5e-3; % Elevation height [m]
param.focus = 18e-3; % Elevation focus [m]

%%Computational grid
%%We can define the computational grid as a uff.linear_scan strcuture. We set different resolution options depending on frequency reference speed of sound.
f_max = 1.2*f0;
lambda_min = c0/f_max;

% mesh resolution, choose one
mesh_resolution='element2'; 
switch mesh_resolution
    case 'element2' % around 50 sec per wave
        dx=param.pitch/2;                                         % 2 elements per pitch 
    case 'element4' % around 6min sec per wave
        dx=param.pitch/4;                                         % 4 elements per pitch 
    otherwise
        error('Not a valid option');
end

% mesh size
PML_size = 20;                                          % size of the PML in grid points
Nx=round(40e-3/dx); Nx=Nx+mod(Nx,2);
Nz=round(30e-3/dx); Nz=Nz+mod(Nz,2);
grid_width=Nx*dx;
grid_depth=Nz*dx;
% domain=uff.linear_scan('x_axis', linspace(-grid_width/2,grid_width/2,Nx).', 'z_axis', linspace(0,grid_depth,Nz).');

kgrid = kWaveGrid(Nz, dx, Nx, dx);
gridz = kgrid.x-kgrid.x_vec(1);
gridx = kgrid.y;

%%Propagation medium
%%We define the medium based by setting the sound speed and density in every pixel of the uff.scan. Here we set an hyperechoic cyst at the center of the domain.
% transparent background
medium.sound_speed = c0*ones(Nz, Nx);   % sound speed [m/s]
medium.density = rho0.*ones(Nz, Nx);      % density [kg/m3]

% include fat layer
fat_std = 3/100;
cn=abs(gridz-(10e-3 + 0.025e-3*sin(2*pi*gridx/5e-3)))<0.5e-3;
rng(1);
medium.sound_speed(cn) = random('normal',1450,1450*fat_std,size(medium.sound_speed(cn)));       % sound speed [m/s]
rng(1);
medium.density(cn) = random('normal',950,950*fat_std,size(medium.density(cn)));               % density [kg/m3]

% include cyst
cyst_std = 3/100;
cx=0; cz=20e-3; cr = 5e-3;
cn=sqrt((gridx-cx).^2+(gridz-cz).^2)<cr;
rng(2);
medium.sound_speed(cn) = random('normal',1540,1540*cyst_std,size(medium.sound_speed(cn)));       % sound speed [m/s]
rng(2);
medium.density(cn) = random('normal',1020,1020*cyst_std,size(medium.density(cn)));               % density [kg/m3]

% % include point
% if true
%     cx=0; cz=20e-3; cr = 0.06125e-3;
%     cn=sqrt((domain.x-cx).^2+(domain.z-cz).^2)<cr;
%     medium.sound_speed(cn) = 1450;       % sound speed [m/s]
%     medium.density(cn) = 1020;           % density [kg/m3]
% end

% attenuation
medium.alpha_coeff = 0.3;  % [dB/(MHz^y cm)]
medium.alpha_power = 1.5;

% show physical map: speed of sound and density
figure;
subplot(1,2,1);
imagesc(gridx(1,:)*1e3,gridz(:,1)*1e3,medium.sound_speed); colormap gray; colorbar; axis equal tight;
xlabel('x [mm]');
ylabel('z [mm]');
title('c_0 [m/s]');
subplot(1,2,2);
imagesc(gridx(1,:)*1e3,gridz(:,1)*1e3,medium.density); colormap gray; colorbar; axis equal tight;
xlabel('x [mm]');
ylabel('z [mm]');
title('\rho [kg/m^3]');

%%Time vector
%%We define the time vector depending on the CFL number, the size of the domain and the mean speed of sound.
cfl=0.3;
t_end=2*sqrt(grid_depth.^2+grid_depth.^2)/mean(medium.sound_speed(:));
kgrid.makeTime(medium.sound_speed,cfl,t_end);
kgrid.Nt=round(kgrid.Nt/1.5);

%%Sequence
%%We define a sequence of plane-waves
% alpha_max=1/2/F_number;                         % maximum angle span [rad]
alpha_max=10/180*pi;
if N>1
    angles=linspace(-alpha_max,alpha_max,N);    % angle vector [rad]
else
    angles = 0;
end
% seq=uff.wave();
% for n=1:N
%     seq(n)=uff.wave();
%     seq(n).apodization = uff.apodization('f_number',1,'window',uff.window.rectangular,'focus',uff.scan('xyz',[0 0 10e-3]));
%     seq(n).source.azimuth=angles(n);
%     seq(n).source.distance=-Inf;
%     seq(n).probe=prb;
%     seq(n).sound_speed=1540;    % reference speed of sound [m/s]
%     seq(n).delay = min(seq(n).delay_values);
%     seq(n).source.plot(fig_handle);
% end


%%Source & sensor mask
%%Based on the uff.probe we find the pixels in the domain that must work as source and sensors.
trans_x = ((0:128-1)-(128-1)/2)*param.pitch;%阵元中心位置
% find the grid-points that match the element
source_pixels={};
element_sensor_index = {};
n=1;
for m=1:param.Nelements
%     figure(111)
%     plot((trans_x(m)+[-param.width/2 param.width/2])*1e3,[0 0],'k+-'); hold on; grid on;
    source_pixels{m}=find(abs(gridx+dx/2-trans_x(m))<=param.width/2 & gridz==0);
    element_sensor_index{m} = n:n+numel(source_pixels{m})-1;
    n=n+numel(source_pixels{m});
end
% figure(111)
% hold off
% sensor mask
sensor.mask = zeros(size(medium.density));
for m=1:param.Nelements
    sensor.mask(source_pixels{m}) = sensor.mask(source_pixels{m}) + 1;
end

% source mask
source.u_mask=sensor.mask;

figure;
h=pcolor(gridx(1,:)*1e3,gridz(:,1)*1e3,source.u_mask); axis equal tight;
title('Source/Sensor mask')
set(h,'edgecolor','none');
set(gca,'YDir','reverse');
xlabel('x [mm]');
ylabel('z [mm]');

%%Delay
delay_values = zeros(param.Nelements,N);
for n=1:N
    if angles(n)~=0
    delay_values(:,n) = -sqrt((trans_x-100*sin(angles(n))).^2+(zeros(size(trans_x))-100*cos(angles(n))).^2)./c0;
    delay_values(:,n) = delay_values(:,n)-min(delay_values(:,n));
    end
end


%%Calculation
%%We are ready to launch the k-Wave calculation
sensor_data =zeros(kgrid.Nt,param.Nelements*2,N);
disp('Launching kWave. This can take a while.');
for n=1:N
    delay=delay_values(:,n);
    denay=round(delay/kgrid.dt);
%     seq(n).delay = seq(n).delay - cycles/f0/2;
    
    % offsets
    tone_burst_offset = [];
    for m=1:param.Nelements
        tone_burst_offset = [tone_burst_offset repmat(denay(m),1,numel(source_pixels{m}))];
    end
    source.ux = toneBurst(1/kgrid.dt, f0, cycles, 'SignalOffset', tone_burst_offset);   % create the tone burst signals
    source.uy = 0.*source.ux;
    source.u_mode ='additive';
    
    % set the input arguements: force the PML to be outside the computational
    % grid; switch off p0 smoothing within kspaceFirstOrder2D
    input_args = {'DataCast', 'gpuArray-single','PMLInside', false, 'PMLSize', PML_size, 'PlotPML', false, 'Smooth', false,'PlotScale',[-1000000 1000000]};
    
    % run the simulation
    sensor_data(:,:,n) = gather(permute(kspaceFirstOrder2D(kgrid, medium, source, sensor, input_args{:}),[2 1]));
end
sensor_data(isnan(sensor_data))=0;

%%Gather element signals
%%After calculaton we combine the signal recorded by the sensors according to the corresponding element
element_data=zeros(numel(kgrid.t_array),param.Nelements,N);
for m=1:param.Nelements
    if  ~isempty(element_sensor_index{m})
        element_data(:,m,:)=bsxfun(@times,sqrt(1./kgrid.t_array).',trapz(kgrid.y(source_pixels{m}),sensor_data(:,element_sensor_index{m},:),2));
    end
end

plot(element_data(:,128,1));
element_data(1:100,:)=0;
plot((element_data(:,1:14:128)/max(element_data(:))+(1:10))',...
 (0:size(element_data,1)-1)*kgrid.dt*1e6,'k')
set(gca,'XTick',1:10,'XTickLabel',int2str((1:14:128)'))
title('RF signals')
xlabel('Element number'), ylabel('time (\mus)')
xlim([0 11])
axis ij square

%%Beamforming
%%To beamform we define a new (coarser) uff.linear_scan. We also define the processing pipeline and launch the beamformer
% prb=uff.linear_array();
% prb.N=128;                  % number of elements
% prb.pitch=300e-6;           % probe pitch in azimuth [m]
% prb.element_width=300e-6;   % element width [m]
% prb.element_height=5000e-6; % element height [m]
% fig_handle = prb.plot([],'Linear array');


param.fs = 1/kgrid.dt;
% txdel = txdelay(0,100,param); % in s
xnum=200;
ynum=200;
I=zeros(200,200,N)
for n=1:3
% txdel = zeros(1,128);
txdel = delay_values(:,n);
IQ = rf2iq(element_data(:,:,n),param.fs,param.fc);
iq = IQ(:,64);
plot(abs(iq))
set(gca,'YColor','none','box','off')
xlabel('time (\mus)')
title('I/Q signal')
% legend({'in-phase','quadrature'})
axis tight

param.fnumber = [];
[xi,zi] = meshgrid(linspace(-2e-2,2e-2,xnum),linspace(0,3e-2,ynum));

bIQ = das(IQ,xi,zi,txdel,param);

I(:,:,n) = bmode(bIQ,40); % log-compressed image

end
figure
imagesc(xi(1,:)*1e2,zi(:,1)*1e2,sum(IU,3))
colormap gray
title('PW-based echo image')
axis equal ij
set(gca,'XColor','none','box','off')
c = colorbar;
c.YTick = [0 255];
c.YTickLabel = {'-40 dB','0 dB'};
ylabel('[cm]')
